/*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014-2015, Timm Linder, Social Robotics Lab, University of Freiburg
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions are met:
*
*  * Redistributions of source code must retain the above copyright notice, this
*    list of conditions and the following disclaimer.
*  * Redistributions in binary form must reproduce the above copyright notice,
*    this list of conditions and the following disclaimer in the documentation
*    and/or other materials provided with the distribution.
*  * Neither the name of the copyright holder nor the names of its contributors
*    may be used to endorse or promote products derived from this software
*    without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <srl_laser_segmentation/jump_distance.h>

namespace srl_laser_segmentation {

JumpDistanceSegmentation::JumpDistanceSegmentation(double jumpDistance) : m_jumpDistance(jumpDistance)
{
}

void JumpDistanceSegmentation::performSegmentation(const std::vector<Point2D>& points, std::vector<srl_laser_segmentation::LaserscanSegment::Ptr>& resultingSegments)
{
    unsigned int segmentCounter = 0;
    srl_laser_segmentation::LaserscanSegment::Ptr currentSegment(new srl_laser_segmentation::LaserscanSegment);
    const Point2D *previousPoint = NULL, *currentPoint = NULL;

    currentSegment->label = segmentCounter;

    for(size_t pointIndex = 0; pointIndex < points.size(); pointIndex++){
        // Skip measurement if it is out-of-range
        const Point2D *newPoint = &points[pointIndex];
        if(!isValidMeasurement(newPoint)) continue;

        previousPoint = currentPoint;
        currentPoint = newPoint;

        if(previousPoint && isJumpBetween(previousPoint, currentPoint)) {
            resultingSegments.push_back(currentSegment);

            currentSegment.reset(new srl_laser_segmentation::LaserscanSegment);
            currentSegment->label = segmentCounter++;
        }

        currentSegment->measurement_indices.push_back(pointIndex);
    }

    // Don't forget storing the last segment
    if(!currentSegment->measurement_indices.empty()) resultingSegments.push_back(currentSegment);
}

bool JumpDistanceSegmentation::isJumpBetween(const Point2D* p1, const Point2D* p2)
{
    const Point2D diff = *p1 - *p2;
    return diff.norm() > m_jumpDistance;
}


} // end of namespace srl_laser_segmentation
